//=================================================================================================
// Copyright (c) 2012, Stefan Kohlbrecher, TU Darmstadt
// All rights reserved.

// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//     * Neither the name of the Simulation, Systems Optimization and Robotics
//       group, TU Darmstadt nor the names of its contributors may be used to
//       endorse or promote products derived from this software without
//       specific prior written permission.

// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//=================================================================================================

#ifndef GOOGLE_MAPPING_MAP_VISUALIZATION_H_
#define GOOGLE_MAPPING_MAP_VISUALIZATION_H_

#include "Map.h"
#include "visualization_msgs/MarkerArray.h"

namespace google_mapping{

/**
  * Largely condensed copied version of the marker-based visualization that is done inside the OctomapServer.
  */
class MapVisualization
{
public:
  MapVisualization(const google_mapping::Map* map)
  {
    octree_ = map->getOcTree();

    ros::NodeHandle pnh("~");

    marker_pub_ = pnh.advertise<visualization_msgs::MarkerArray>("occupied_cells_vis_array", 1, false);

    default_color_.r = 0.0;
    default_color_.g = 0.0;
    default_color_.b = 1.0;
    default_color_.a = 1.0;
  }

  void publishVisualization()
  {
    visualization_msgs::MarkerArray occupiedNodesVis;
    this->fillMarkerArray(occupiedNodesVis);
    marker_pub_.publish(occupiedNodesVis);
  }

  void fillMarkerArray(visualization_msgs::MarkerArray& occupiedNodesVis){


    bool publishMarkerArray = true;
    bool m_useHeightMap = true;
    double m_colorFactor = 0.8;

    //visualization_msgs::MarkerArray occupiedNodesVis;
    occupiedNodesVis.markers.resize(octree_->getTreeDepth()+1);


    for (octomap::OcTree::iterator it = octree_->begin(octree_->getTreeDepth()),
         end = octree_->end(); it != end; ++it)
    {

      if (octree_->isNodeOccupied(*it)){
        double z = it.getZ();
        //if (z > m_occupancyMinZ && z < m_occupancyMaxZ)
        {
          //double size = it.getSize();
          double x = it.getX();
          double y = it.getY();


          if (publishMarkerArray){
            unsigned idx = it.getDepth();
            assert(idx < occupiedNodesVis.markers.size());

            geometry_msgs::Point cubeCenter;
            cubeCenter.x = x;
            cubeCenter.y = y;
            cubeCenter.z = z;

            occupiedNodesVis.markers[idx].points.push_back(cubeCenter);
            if (m_useHeightMap){
              double minX, minY, minZ, maxX, maxY, maxZ;
              octree_->getMetricMin(minX, minY, minZ);
              octree_->getMetricMax(maxX, maxY, maxZ);

              double h = (1.0 - std::min(std::max((cubeCenter.z-minZ)/ (maxZ - minZ), 0.0), 1.0)) *m_colorFactor;
              occupiedNodesVis.markers[idx].colors.push_back(heightMapColor(h));
            }
          }
        }
      }
    }


    if (publishMarkerArray){

      for (unsigned i= 0; i < occupiedNodesVis.markers.size(); ++i){
        double size = octree_->getNodeSize(i);

        occupiedNodesVis.markers[i].header.frame_id = "/map";
        occupiedNodesVis.markers[i].header.stamp = ros::Time::now();
        occupiedNodesVis.markers[i].ns = "map";
        occupiedNodesVis.markers[i].id = i;
        occupiedNodesVis.markers[i].type = visualization_msgs::Marker::CUBE_LIST;
        occupiedNodesVis.markers[i].scale.x = size;
        occupiedNodesVis.markers[i].scale.y = size;
        occupiedNodesVis.markers[i].scale.z = size;
        occupiedNodesVis.markers[i].color = default_color_;

        //std::cout << " i: " << i << " " << occupiedNodesVis.markers[i].points.size();

        if (occupiedNodesVis.markers[i].points.size() > 0)
          occupiedNodesVis.markers[i].action = visualization_msgs::Marker::ADD;
        else
          occupiedNodesVis.markers[i].action = visualization_msgs::Marker::DELETE;
      }


      //marker_pub_.publish(occupiedNodesVis);
    }
  }

  std_msgs::ColorRGBA heightMapColor(double h) {

    std_msgs::ColorRGBA color;
    color.a = 1.0;
    // blend over HSV-values (more colors)

    double s = 1.0;
    double v = 1.0;

    h -= floor(h);
    h *= 6;
    int i;
    double m, n, f;

    i = floor(h);
    f = h - i;
    if (!(i & 1))
      f = 1 - f; // if i is even
    m = v * (1 - s);
    n = v * (1 - s * f);

    switch (i) {
    case 6:
    case 0:
      color.r = v; color.g = n; color.b = m;
      break;
    case 1:
      color.r = n; color.g = v; color.b = m;
      break;
    case 2:
      color.r = m; color.g = v; color.b = n;
      break;
    case 3:
      color.r = m; color.g = n; color.b = v;
      break;
    case 4:
      color.r = n; color.g = m; color.b = v;
      break;
    case 5:
      color.r = v; color.g = m; color.b = n;
      break;
    default:
      color.r = 1; color.g = 0.5; color.b = 0.5;
      break;
    }
    return color;
  }



protected:
  const octomap::OcTree* octree_;
  ros::Publisher marker_pub_;
  std_msgs::ColorRGBA default_color_;
};

}

#endif
